#include "eigen3/Eigen/Dense"
#include <iostream>
#include "opencv2/opencv.hpp"

using namespace std;

void point_inside_polygon(float* pts,int pts_num, float* polygon_vertices,int polygon_vertices_nu,
                        bool * is_in_box )
{
    //循环第i点，判断是否在polygon中，更新is_in_box
    for (int i=0;i<pts_num;i++)
    {
        float x = pts[i*2+0];
        float y = pts[i*2+1];
        printf("i %d pts x,y  %f %f \n",i,x,y);

        int j = polygon_vertices_nu-1;
        for(int k=0;k<polygon_vertices_nu;k++)
        {
            float xk = polygon_vertices[2*k+0];
            float yk = polygon_vertices[2*k+1];

            float xj = polygon_vertices[2*j+0];
            float yj = polygon_vertices[2*j+1];

            printf("vertices pts k x,y %f %f \n",xk,yk);
            printf("vertices pts j x,y %f %f \n",xj,yj);
            bool is_mid_y = (yk<y && y<=yj) || ( yj < y && y <= yk);
            if(is_mid_y)
            {
                printf("is_mid_y pts %f %f \n",x, y);
                bool is_mid_x = x > ((xj - xk) * (y - yk) / (yj - yk) + xk);
                if (is_mid_x)
                {
                    printf("is_mid_x %f \n",(xj - xk) * (y - yk) / (yj - yk) + xk);
                    is_in_box[i]=!is_in_box[i];
                }
            }
            j=k;
        }
        printf("in box %d \n",is_in_box[i]);
    }
}

int main() {
    cv::Mat im=cv::Mat::zeros(cv::Size(500,500),CV_8UC3);
    
    //draw vertices
    Eigen::Matrix<int,4,2> polygon_vertices;
    polygon_vertices << 1, 1, 4, 2,
                        3, 4, 2, 4;
    polygon_vertices*=100;
    cout << polygon_vertices << endl;
    // cout<<polygon_vertices.size()<<endl;

    //need copy Matrix to float[]
    float *polygon_vertices_f = new float[4*2];
    for(int i=0;i<polygon_vertices.rows();i++)
    {
        cv::Scalar blue = cv::Scalar(255,0,0);
        cv::circle(im,cv::Point(polygon_vertices(i,0),polygon_vertices(i,1)),1,blue,1,1);
        polygon_vertices_f[i*2+0]=polygon_vertices(i,0);
        polygon_vertices_f[i*2+1]=polygon_vertices(i,1);

        //draw line
        cv::Point p1 =cv::Point(polygon_vertices(i,0),polygon_vertices(i,1));
        cv::Point p2;
        if (i+1==polygon_vertices.rows())
            p2 =cv::Point(polygon_vertices(0,0),polygon_vertices(0,1));
        else
            p2 =cv::Point(polygon_vertices(i+1,0),polygon_vertices(i+1,1));
        cv::line(im,p1,p2,blue,1,1);
    }

    //draw pts
    Eigen::Matrix<int,5,2> pts;
    pts << 2, 2, 
            3, 3, 
            3, 2,
            4, 3,
            1, 3;
    pts*=100;
    // cout << polygon_vertices(1,1) << endl;
    float *pts_f=new float[2*5];
    for(int i=0;i<pts.rows();i++)
    {
        // cv::Scalar red = cv::Scalar(0,0,255);
        // cv::circle(im,cv::Point(pts(i,0),pts(i,1)),1,red,1,1);
        pts_f[2*i+0]=pts(i,0);
        pts_f[2*i+1]=pts(i,1);
    }
    // cv::imwrite("res.jpg",im);

    //filter pts in box
    // int *pts_p = pts.data();
    // int *polygon_vertices_p = polygon_vertices.data();
    for(int i=0;i<4;i++)
        printf("polygon_vertices_f i %f %f \n",polygon_vertices_f[i*2+0],polygon_vertices_f[i*2+1]);

    bool *is_in_box = new bool[pts.rows()]{0};
    point_inside_polygon(pts_f,5,polygon_vertices_f,4,is_in_box);
    //draw in box is green, out is red
    for(int i=0;i<pts.rows();i++)
    {
        cv::Scalar red = cv::Scalar(0,0,255);
        cv::Scalar green = cv::Scalar(0,255,0);
        if (is_in_box[i])
            cv::circle(im,cv::Point(pts(i,0),pts(i,1)),1,green,1,1);
        else
            cv::circle(im,cv::Point(pts(i,0),pts(i,1)),1,red,1,1);
    }

    cv::flip(im,im,0);
    cv::imwrite("res.jpg",im);
    return 0;

}
